﻿using System;
using System.Collections.Generic;
using QSharp.Scheme.Mathematics;

namespace QSharp.Signal.Classic
{
    /// <summary>
    ///  A static class that provides DCT algorithms
    /// </summary>
    public static class DctMethods
    {
        #region Methods

        /// <summary>
        ///  This produces type-II DCT of the given input
        /// </summary>
        /// <param name="input">The input sequence</param>
        /// <param name="output">The type-II DCT of the input</param>
        public static void DctIi(this IList<double> input, IList<double> output)
        {
            var n = input.Count;
            var a = Math.PI/n;
            for (var k = 0; k < n; k++)
            {
                var d = 0.0;
                for (var i = 0; i < n; i++)
                {
                    d += input[i]*Math.Cos(a*(i + 0.5)*k);
                }
                output[k] = d;
            }
        }

        /// <summary>
        ///  This produces the inverse of a type-II DCT
        /// </summary>
        /// <param name="input">The input sequence</param>
        /// <param name="output">The inverse type-II DCT of the input</param>
        public static void IdctIi(this IList<double> input, IList<double> output)
        {
            input.DctIii(output);
            var a = 2.0/output.Count;
            for (var i = 0; i < output.Count; i++)
            {
                output[i] *= a;
            }
        }

        /// <summary>
        ///  This produces the equivalent result as DctIi at an improved speed using FFT
        /// </summary>
        /// <param name="input">The input sequence</param>
        /// <param name="output">The type-II DCT of the input</param>
        public static void FctIiFft(this IList<double> input, IList<double> output)
        {
            var n = input.Count;
            var nfft = n*4;
            var fftInput = new Complex[nfft];
            for (var i = 0; i < n; i++)
            {
                fftInput[i*2 + 1] = fftInput[nfft - 1 - i*2] = input[i];
                fftInput[i*2] = fftInput[nfft - 2 - i*2] = 0;
            }
            var fftOutput = new Complex[nfft];
            var fn = new DftMethods.FactorAidedInteger(nfft);
            var order = fn.FftReorder();
            fftInput.FftNonRecursive(fftOutput, fn, order);
            for (var k = 0; k < n; k++)
            {
                output[k] = 0.5*fftOutput[k].Real;
            }
        }

        /// <summary>
        ///  This produces the equivalent result as DctIi at an improved speed using FFT,
        ///  this version is mostly similar and different in that it zeroed the latter half
        /// </summary>
        /// <param name="input">The input sequence</param>
        /// <param name="output">The type-II DCT of the input</param>
        public static void FctIiFft2(this IList<double> input, IList<double> output)
        {
            var n = input.Count;
            var nfft = n * 4;
            var fftInput = new Complex[nfft];
            for (var i = 0; i < n; i++)
            {
                fftInput[i * 2 + 1] = input[i];
                fftInput[nfft - 1 - i*2] = 0;
                fftInput[i * 2] = fftInput[nfft - 2 - i * 2] = 0;
            }
            var fftOutput = new Complex[nfft];
            var fn = new DftMethods.FactorAidedInteger(nfft);
            var order = fn.FftReorder();
            fftInput.FftNonRecursive(fftOutput, fn, order);
            for (var k = 0; k < n; k++)
            {
                output[k] = fftOutput[k].Real;
            }
        }

        /// <summary>
        ///  This produces the inverse of a type-II DCT using FFT
        /// </summary>
        /// <param name="input">The input sequence</param>
        /// <param name="output">The inverse type-II DCT of the input</param>
        public static void IfctIiFft(this IList<double> input, IList<double> output)
        {
            input.FctIiiFft(output);
            var a = 2.0/output.Count;
            for (var i = 0; i < output.Count; i++)
            {
                output[i] *= a;
            }
        }

        /// <summary>
        ///  This produces type-III dct of the given input
        /// </summary>
        /// <param name="input">The input sequence</param>
        /// <param name="output">The type-III DCT of the input</param>
        public static void DctIii(this IList<double> input, IList<double> output)
        {
            var n = input.Count;
            var a = Math.PI/n;
            var offset = 0.5*input[0];
            for (var k = 0; k < n; k++)
            {
                var d = offset;
                for (var i = 1; i < n; i++)
                {
                    d += input[i]*Math.Cos(a*i*(k + 0.5));
                }
                output[k] = d;
            }
        }

        /// <summary>
        ///  This produces the inverse of a type-III DCT
        /// </summary>
        /// <param name="input">The input sequence</param>
        /// <param name="output">The inverse type-III DCT of the input</param>
        public static void IdctIii(this IList<double> input, IList<double> output)
        {
            input.DctIi(output);
            var a = 2.0 / output.Count;
            for (var i = 0; i < output.Count; i++)
            {
                output[i] *= a;
            }
        }

        /// <summary>
        ///  This produces the equivalent result as DctIii at an improved speed using FFT
        /// </summary>
        /// <param name="input">The input sequence</param>
        /// <param name="output">The type-II DCT of the input</param>
        public static void FctIiiFft(this IList<double> input, IList<double> output)
        {
            var n = input.Count;
            var nfft = n*4;
            var fftInput = new Complex[nfft];
            for (var i = 0; i < n; i++)
            {
                fftInput[i] = input[i];
            }
            for (var i = n; i < nfft; i++)
            {
                fftInput[i] = 0;
            }
            var fftOutput = new Complex[nfft];
            var fn = new DftMethods.FactorAidedInteger(nfft);
            var order = fn.FftReorder();
            fftInput.FftNonRecursive(fftOutput, fn, order);
            var offset = -input[0]*0.5;
            for (var k = 0; k < n; k++)
            {
                output[k] = fftOutput[2*k + 1].Real + offset;
            }
        }

        /// <summary>
        ///  This produces the inverse of a type-III DCT using FFT
        /// </summary>
        /// <param name="input">The input sequence</param>
        /// <param name="output">The inverse type-III DCT of the input</param>
        public static void IfctIiiFft(this IList<double> input, IList<double> output)
        {
            input.FctIiFft(output);
            var a = 2.0 / output.Count;
            for (var i = 0; i < output.Count; i++)
            {
                output[i] *= a;
            }
        }

        /// <summary>
        ///  Type-IV DCT (orthogonal)
        /// </summary>
        /// <param name="input">The input sequence</param>
        /// <param name="output">The type-IV DCT of the input sequence</param>
        public static void DctIv(this IList<double> input, IList<double> output)
        {
            input.DctIvCore(0, 1, Math.PI/input.Count, output);            
        }

        /// <summary>
        ///  Type-IV DCT (orthogonal) using FFT
        /// </summary>
        /// <param name="input">The input sequence</param>
        /// <param name="output">The type-IV DCT of the input sequence</param>
        public static void FctIvFft(this IList<double> input, IList<double> output)
        {
            var n = input.Count;
            var nfft = n * 8;
            var fftInput = new Complex[nfft];
            for (var i = 0; i < n; i++)
            {
                fftInput[2*i+1] = input[i];
                fftInput[2*i] = 0;
            }
            for (var i = 2*n; i < nfft; i++)
            {
                fftInput[i] = 0;
            }
            var fftOutput = new Complex[nfft];
            var fn = new DftMethods.FactorAidedInteger(nfft);
            var order = fn.FftReorder();
            fftInput.FftNonRecursive(fftOutput, fn, order);
            for (var k = 0; k < n; k++)
            {
                output[k] = fftOutput[2 * k + 1].Real;
            }
        }

        private static void DctIvCore(this IList<double> input, int start, int skip, double a, IList<double> output)
        {
            var ni = input.Count;
            var n = output.Count;
            for (var k = 0; k < n; k++)
            {
                var d = 0.0;
                var j = 0;
                for (var i = start; i < ni; i += skip, j++)
                {
                    d += input[i]*Math.Cos(a*(k + 0.5)*(j + 0.5));
                }
                output[k] = d;
            }
        }

#if false

        /// <summary>
        ///  Type-IV DCT implemented using recursive method
        ///  Warning: This method is mathematically crippled even slower than the normal one and thereby not suitable for fast caluculation
        /// </summary>
        /// <param name="input">The input sequence</param>
        /// <param name="output">The output sequence</param>
        public static void FctIvRecursive(this IList<double> input, IList<double> output)
        {
            var n = new DftMethods.FactorAidedInteger(input.Count);
            input.FctIvRecursiveCore(n, 0, 1, output);
        }

        private static void DctIvFCore(this IList<double> input, int start, int skip, double a, IList<double> output)
        {
            var ni = input.Count;
            var n = output.Count;
            for (var k = 0; k < n; k++)
            {
                var d = 0.0;
                var j = 0;
                for (var i = start; i < ni; i += skip, j++)
                {
                    var v = input[i] * Math.Cos(a * (k + 0.5) * (j + 0.5));
                    if (j % 2 != 0)
                    {
                        v = -v;
                    }
                    d += v;
                }
                output[k] = d;
            }
        }

        private static void DstIvCore(this IList<double> input, int start, int skip, double a, IList<double> output)
        {
            var ni = input.Count;
            var n = output.Count;
            for (var k = 0; k < n; k++)
            {
                var d = 0.0;
                var j = 0;
                for (var i = start; i < ni; i += skip, j++)
                {
                    d += input[i]*Math.Sin(a*(k + 0.5)*(j + 0.5));
                }
                output[k] = d;
            }
        }

        private static void DstIvFCore(this IList<double> input, int start, int skip, double a, IList<double> output)
        {
            var ni = input.Count;
            var n = output.Count;
            for (var k = 0; k < n; k++)
            {
                var d = 0.0;
                var j = 0;
                for (var i = start; i < ni; i += skip, j++)
                {
                    var v = input[i] * Math.Sin(a * (k + 0.5) * (j + 0.5));
                    if (j % 2 != 0)
                    {
                        v = -v;
                    }
                    d += v;
                }
                output[k] = d;
            }
        }


        private static void FctIvRecursiveCore(this IList<double> input,
                                               DftMethods.FactorAidedInteger n, int start, int skip,
                                               IList<double> output)
        {
            var n0 = n.N;
            var n1 = n.Decompose();
            var n2 = n.N;
            var a = Math.PI/n0;

            if (n1 > 1 && n2 > 1)
            {
                for (var k = 0; k < n0; k++)
                {
                    output[k] = 0;
                }

                var b = 0.25*a;
                for (var r = 0; r < n1; r++)
                {
                    int newStart, newSkip;

                    DftMethods.GetSubsequenceIndexGenerator(start, skip, r, n1, out newStart, out newSkip);

                    // N2-point DCT and DST
                    var tmpDct = new double[n2];
                    var tmpDst = new double[n2];
                    var tmpDctF = new double[n2];
                    var tmpDstF = new double[n2];
                    input.FctIvRecursiveCore(n, newStart, newSkip, tmpDct);
                    input.FstIvRecursiveCore(n, newStart, newSkip, tmpDst);
                    input.FctIvFRecursiveCore(n, newStart, newSkip, tmpDctF);
                    input.FstIvFRecursiveCore(n, newStart, newSkip, tmpDstF);

                    var c = b*(2*r + 1 - n1);
                    for (var k = 0; k < n0; k++)
                    {
                        var k2 = k%n2;
                        var l = k/n2;
                        var phi = c*(2*k + 1);
                        var lMod4 = l%4;
                        switch (lMod4)
                        {
                            case 0:
                                output[k] += tmpDct[k2] * Math.Cos(phi) - tmpDst[k2] * Math.Sin(phi);
                                break;
                            case 2:
                                output[k] += -tmpDct[k2] * Math.Cos(phi) + tmpDst[k2] * Math.Sin(phi);
                                break;
                            case 1:
                                output[k] += -tmpDstF[k2] * Math.Cos(phi) - tmpDctF[k2] * Math.Sin(phi);
                                break;
                            case 3:
                                output[k] += tmpDstF[k2] * Math.Cos(phi) + tmpDctF[k2] * Math.Sin(phi);
                                break;
                        }
                    }
                }
            }
            else
            {
                input.DctIvCore(start, skip, a, output);
            }

            n.Restore(n1);
        }

        private static void FstIvRecursiveCore(this IList<double> input,
                                               DftMethods.FactorAidedInteger n, int start, int skip,
                                               IList<double> output)
        {
            var n0 = n.N;
            var n1 = n.Decompose();
            var n2 = n.N;
            var a = Math.PI/n0;

            if (n1 > 1 && n2 > 1)
            {
                for (var k = 0; k < n0; k++)
                {
                    output[k] = 0;
                }

                var b = 0.25*a;
                for (var r = 0; r < n1; r++)
                {
                    int newStart, newSkip;

                    DftMethods.GetSubsequenceIndexGenerator(start, skip, r, n1, out newStart, out newSkip);

                    // N2-point DCT and DST
                    var tmpDct = new double[n2];
                    var tmpDst = new double[n2];
                    var tmpDctF = new double[n2];
                    var tmpDstF = new double[n2];
                    input.FctIvRecursiveCore(n, newStart, newSkip, tmpDct);
                    input.FstIvRecursiveCore(n, newStart, newSkip, tmpDst);
                    input.FctIvFRecursiveCore(n, newStart, newSkip, tmpDctF);
                    input.FstIvFRecursiveCore(n, newStart, newSkip, tmpDstF);

                    var c = b*(2*r + 1 - n1);
                    for (var k = 0; k < n0; k++)
                    {
                        var k2 = k%n2;
                        var l = k/n2;
                        var phi = c*(2*k + 1);
                        var lMod4 = l % 4;
                        switch (lMod4)
                        {
                            case 0:
                                output[k] += tmpDst[k2] * Math.Cos(phi) + tmpDct[k2] * Math.Sin(phi);
                                break;
                            case 2:
                                output[k] += -tmpDst[k2] * Math.Cos(phi) - tmpDct[k2] * Math.Sin(phi);
                                break;
                            case 1:
                                output[k] += tmpDctF[k2] * Math.Cos(phi) - tmpDstF[k2] * Math.Sin(phi);
                                break;
                            case 3:
                                output[k] += -tmpDctF[k2] * Math.Cos(phi) + tmpDstF[k2] * Math.Sin(phi);
                                break;
                        }
                    }
                }
            }
            else
            {
                input.DstIvCore(start, skip, a, output);
            }

            n.Restore(n1);
        }

        private static void FctIvFRecursiveCore(this IList<double> input,
                                                DftMethods.FactorAidedInteger n, int start, int skip,
                                                IList<double> output)
        {
            var n0 = n.N;
            var n1 = n.Decompose();
            var n2 = n.N;
            var a = Math.PI / n0;

            if (n1 > 1 && n2 > 1)
            {
                for (var k = 0; k < n0; k++)
                {
                    output[k] = 0;
                }

                var b = 0.25 * a;
                for (var r = 0; r < n1; r++)
                {
                    int newStart, newSkip;

                    DftMethods.GetSubsequenceIndexGenerator(start, skip, r, n1, out newStart, out newSkip);

                    // N2-point DCT and DST
                    var tmpDct1 = new double[n2];
                    var tmpDst1 = new double[n2];
                    var tmpDct2 = new double[n2];
                    var tmpDst2 = new double[n2];
                    if (n1%2 == 0)
                    {
                        input.FctIvRecursiveCore(n, newStart, newSkip, tmpDct1);
                        input.FstIvRecursiveCore(n, newStart, newSkip, tmpDst1);
                        input.FctIvFRecursiveCore(n, newStart, newSkip, tmpDct2);
                        input.FstIvFRecursiveCore(n, newStart, newSkip, tmpDst2);
                    }
                    else
                    {
                        input.FctIvRecursiveCore(n, newStart, newSkip, tmpDct2);
                        input.FstIvRecursiveCore(n, newStart, newSkip, tmpDst2);
                        input.FctIvFRecursiveCore(n, newStart, newSkip, tmpDct1);
                        input.FstIvFRecursiveCore(n, newStart, newSkip, tmpDst1);
                    }

                    var c = b*(2*r + 1 - n1);
                    var rIsOdd = r%2 != 0;
                    for (var k = 0; k < n0; k++)
                    {
                        var k2 = k % n2;
                        var l = k / n2;
                        var phi = c * (2 * k + 1);
                        var lMod4 = l % 4;
                        var d = 0.0;
                        switch (lMod4)
                        {
                            case 0:
                                d = tmpDct1[k2] * Math.Cos(phi) - tmpDst1[k2] * Math.Sin(phi);
                                break;
                            case 2:
                                d = -tmpDct1[k2] * Math.Cos(phi) + tmpDst1[k2] * Math.Sin(phi);
                                break;
                            case 1:
                                d = -tmpDst2[k2] * Math.Cos(phi) - tmpDct2[k2] * Math.Sin(phi);
                                break;
                            case 3:
                                d = tmpDst2[k2] * Math.Cos(phi) + tmpDct2[k2] * Math.Sin(phi);
                                break;
                        }
                        if (rIsOdd)
                        {
                            d = -d;
                        }
                        output[k] += d;
                    }
                }
            }
            else
            {
                input.DctIvFCore(start, skip, a, output);
            }

            n.Restore(n1);
        }

        private static void FstIvFRecursiveCore(this IList<double> input,
                                                DftMethods.FactorAidedInteger n, int start, int skip,
                                                IList<double> output)
        {
            var n0 = n.N;
            var n1 = n.Decompose();
            var n2 = n.N;
            var a = Math.PI / n0;

            if (n1 > 1 && n2 > 1)
            {
                for (var k = 0; k < n0; k++)
                {
                    output[k] = 0;
                }

                var b = 0.25 * a;
                for (var r = 0; r < n1; r++)
                {
                    int newStart, newSkip;

                    DftMethods.GetSubsequenceIndexGenerator(start, skip, r, n1, out newStart, out newSkip);

                    // N2-point DCT and DST
                    var tmpDct1 = new double[n2];
                    var tmpDst1 = new double[n2];
                    var tmpDct2 = new double[n2];
                    var tmpDst2 = new double[n2];
                    if (n1%2 == 0)
                    {
                        input.FctIvRecursiveCore(n, newStart, newSkip, tmpDct1);
                        input.FstIvRecursiveCore(n, newStart, newSkip, tmpDst1);
                        input.FctIvFRecursiveCore(n, newStart, newSkip, tmpDct2);
                        input.FstIvFRecursiveCore(n, newStart, newSkip, tmpDst2);
                    }
                    else
                    {
                        input.FctIvRecursiveCore(n, newStart, newSkip, tmpDct2);
                        input.FstIvRecursiveCore(n, newStart, newSkip, tmpDst2);
                        input.FctIvFRecursiveCore(n, newStart, newSkip, tmpDct1);
                        input.FstIvFRecursiveCore(n, newStart, newSkip, tmpDst1);
                    }

                    var c = b * (2 * r + 1 - n1);
                    var rIsOdd = r%2 != 0;
                    for (var k = 0; k < n0; k++)
                    {
                        var k2 = k % n2;
                        var l = k / n2;
                        var phi = c * (2 * k + 1);
                        var lMod4 = l % 4;
                        var d = 0.0;
                        switch (lMod4)
                        {
                            case 0:
                                d = tmpDst1[k2] * Math.Cos(phi) + tmpDct1[k2] * Math.Sin(phi);
                                break;
                            case 2:
                                d = -tmpDst1[k2] * Math.Cos(phi) - tmpDct1[k2] * Math.Sin(phi);
                                break;
                            case 1:
                                d = tmpDct2[k2] * Math.Cos(phi) - tmpDst2[k2] * Math.Sin(phi);
                                break;
                            case 3:
                                d = tmpDct2[k2] * Math.Cos(phi) - tmpDst2[k2] * Math.Sin(phi);
                                break;
                        }
                        if (rIsOdd)
                        {
                            d = -d;
                        }
                        output[k] += d;
                    }
                }
            }
            else
            {
                input.DstIvFCore(start, skip, a, output);
            }

            n.Restore(n1);
        }
#endif

        #endregion
    }
}
